//
// Created by admin on 2024/8/15.
//

#include "Modbus.h"
#include <string.h>


static uint16_t CRC_checkout_16(uint8_t *data, uint32_t len) {
    if (data == NULL) {
        return 0;
    }

    uint16_t wcrc = 0xFFFF;
    uint8_t  temp;

    for (uint32_t i = 0; i < len; i++) {
        temp = *data & 0X00FF;
        data++;

        wcrc = wcrc ^ temp;
        for (char j = 0; j < 8; j++) {
            if (wcrc & 0X0001) {
                wcrc >>= 1;
                wcrc ^= 0XA001;
            } else {
                wcrc >>= 1;
            }
        }
    }

    return wcrc;
}


int Modbus::begin(uint8_t start, uint8_t dev_addr, uint32_t timeout) {
    _startByte = start;
    _devAddr   = dev_addr;
    _timeout   = timeout;
    crc16      = CRC_checkout_16;

    return 0;
}

void Modbus::setCRC(Modbus::Crc16Callback_t callback) {

    if (callback != NULL)
        crc16 = callback;
}


int Modbus::sendFrame(uint8_t fun_code, uint8_t *data, uint32_t len) {
    if (data == NULL || len == 0) {
        return 0;
    }

    RTUFrame_t *frame    = (RTUFrame_t *) txBuffer;
    uint32_t   frame_len = RTU_FORMAT_LEN + len;

    frame->startByte = _startByte;
    frame->devAddr   = _devAddr;
    frame->funCode   = fun_code;
    frame->dataLen   = (uint8_t) len;

    memcpy(txBuffer + RTU_HEAD_LEN, data, len);

    uint16_t *crc = (uint16_t *) (txBuffer + RTU_HEAD_LEN + len);

    *crc = crc16(txBuffer, RTU_HEAD_LEN + len);


    return (int) Serial->write(txBuffer, frame_len);
}

int Modbus::sendFrame(uint8_t fun_code, uint8_t data) {
    return sendFrame(fun_code, &data, 1);
}

int Modbus::sendFrame(RTUFrame_t *frame) {
    return sendFrame(frame->funCode, frame->data, frame->dataLen);
}


int Modbus::readFrame(Modbus::RTUFrame_t *result) {
    if (Serial->available() < RTU_FORMAT_LEN) {
        return LENGTH_ERROR;
    }

    RTUFrame_t *frame = (RTUFrame_t *) rxBuffer;

    Serial->read(rxBuffer, RTU_HEAD_LEN);

    /* 起始字节校验 */
    if (frame->startByte != _startByte) {
        return START_ERROR;
    }

    /* 设备地址校验 */
    if (frame->devAddr != _devAddr) {
        return DEV_ERROR;
    }

    uint32_t frame_len = frame->dataLen + RTU_FORMAT_LEN;
    auto     rd_len    = Serial->read(rxBuffer + RTU_HEAD_LEN, frame_len - RTU_HEAD_LEN);

    /* 长度校验 */
    if (frame_len != rd_len) {
        return LENGTH_ERROR;
    }

    /* crc校验 */
    uint16_t calc_crc = crc16(rxBuffer, frame_len - RTU_TAIL_LEN);
    uint16_t crc      = *(uint16_t *) (rxBuffer + frame->dataLen + RTU_HEAD_LEN);
    if (crc != calc_crc) {
        return CRC_ERROR;
    }

    /* 拷贝数据 */
    memcpy(result, frame, RTU_HEAD_LEN);
    result->data = rxBuffer + RTU_HEAD_LEN;
    result->crc  = crc;

    return RTU_OK;
}

int Modbus::readFrame(uint8_t fun_code, uint8_t *data, uint32_t len) {
    int ret = RTU_OK;

    RTUFrame_t frame;

    ret = readFrame(&frame);

    if (frame.funCode != fun_code) {
        return CODE_ERROR;
    }

    if (ret == RTU_OK && data != NULL) {
        memcpy(data, frame.data, frame.dataLen > len ? len : frame.dataLen);
    }

    return ret;
}

int Modbus::timeReadFrame(RTUFrame_t *frame) {
    auto startTime = millis();

    int len;
    do {
        len = Serial->available();
        if (len > RTU_FORMAT_LEN) break;
    } while (millis() - startTime < _timeout);

    if (millis() - startTime > _timeout) {
        return READ_TIMEOUT;
    }

    return readFrame(frame);
}


Modbus::Res_t Modbus::requestSend(uint8_t fun_code, uint8_t *data, uint32_t len) {
    Res_t ret;
    RTUFrame_t frame;

    int tx_size = sendFrame(fun_code, data, len);

    if(tx_size < len + RTU_FORMAT_LEN){

    }

    ret = (Res_t) timeReadFrame(&frame);

    if(ret != RTU_OK){
        return ret;
    }

    if(frame.funCode != fun_code){
        ret = CODE_ERROR;
    }

    return ret;
}

